package com.robb19y.rgbdscanner.icp;

import java.util.ArrayList;
import java.util.Arrays;

import com.robb19y.rgbdscanner.core.RGBDData;

public class ICPAlgorithm {
	static {
	      System.loadLibrary("icp"); 	                                   
	   }	 
	  
	private native double[] icp(double[] M,double[] T);
	
	public void pointCloudRegistration(ArrayList<RGBDData> model,ArrayList<RGBDData> data) {
		double[] M = new double[3*model.size()];
		double[] T = new double[3*data.size()];
		double[] pos;
		for(int i = 0; i < model.size(); i++){
			pos = model.get(i).getPosition();
			M[i*3+0] = pos[0];
			M[i*3+1] = pos[1];
			M[i*3+2] = pos[2];
		}
		for(int i = 0; i < data.size(); i++){
			pos = data.get(i).getPosition();
			T[i*3+0] = pos[0];
			T[i*3+1] = pos[1];
			T[i*3+2] = pos[2];
		}
		double[] R = icp(M,T);
		
		for(int i = 0; i < data.size(); i++){
			pos = new double[]{R[3*i+0],R[3*i+1],R[3*i+2]};
			data.get(i).setPosition(pos);
		}
	}
	   
	   public static void main(String[] args){
		   int k = 0;
		   double[] M = new double[3*10000];
		   double[] T = new double[3*10000];
		   for (double x=-2; x<2; x+=0.04) {
			    for (double y=-2; y<2; y+=0.04) {
			      double z=5*x*Math.exp(-x*x-y*y);
			      
				M[k*3+0] = x;
			      M[k*3+1] = y;
			      M[k*3+2] = z;
			      T[k*3+0] = x-1;
			      T[k*3+1] = y-1;
			      T[k*3+2] = z-1;
			      k++;
			    }
			  }
		   double[] r = new ICPAlgorithm().icp(M,T);
		   System.out.println(M[2]);
		   System.out.println(T[2]);
		   System.out.println(r[2]);
	   }
	

}
